#include <cstdio>
#include <iostream>
#include <vector>
#include <limits>
#include "utils.h"

struct Point {
	int x, y;
};

/**
 * @brief 计算方差
 *
 * @param cen_x 机器人中心x坐标
 * @param cen_y 机器人中心y坐标
 * @param points 在全景图像中，离机器人距离相同的采样点
 *
 * @return 方差
 */
static float getError(int cen_x, int cen_y, const std::vector<Point> &points)
{
	float dist;
	std::vector<float> dists;
	float average = 0.0f;
	float ret = 0.0f;

	for (std::vector<Point>::const_iterator i = points.begin();
			i != points.end(); ++i) {
		dist = pixToDistance((*i).x, (*i).y, cen_x, cen_y);
		dists.push_back(dist);
		average += dist;
	}
	average /= points.size();

	for (std::vector<float>::const_iterator i = dists.begin();
			i != dists.end(); ++i) {
		ret += (average - *i) * (average - *i);
	}

	return ret;
}

int main(int argc, char **argv)
{
	FILE *file;
	char buf[50];
	std::vector<Point> points;
	Point point;
	const int START_X = 200, START_Y = 200;
	const int END_X = 400, END_Y = 400;
	float error;
	int min_x, min_y;
	float min_error = std::numeric_limits<float>::max();

	// 打开文件
	file = fopen("find_center.txt", "r");
	if (!file) {
		std::cerr << "can't open file omnidirection.txt" << std::endl;
		return 1;
	}

	// 读取文件
	while (fgets(buf, sizeof(buf), file)) {
		if (buf[0] == '#') {
			continue;
		}
		sscanf(buf, "%d %d", &(point.x), &(point.y));
		points.push_back(point);
	}

	// 关闭文件
	fclose(file);

	// 计算最小方差
	for (int x = START_X; x < END_X; ++x) {
		for (int y = START_Y; y < END_Y; ++y) {
			error = getError(x, y, points);
			if (error < min_error) {
				min_error = error;
				min_x = x;
				min_y = y;
			}
		}
	}
	std::cout << "error: " << min_error << std::endl;
	std::cout << "center: (" << min_x << ", " << min_y << ")" << std::endl;
	std::cout << "Press CTRL + C to continue...";
	std::cin >> min_x;

	return 0;
}
